/**
 * @file serial_driver.cpp
 * @author circleup@foxmail.com
 * @brief 串口处理相关
 * @version 0.1
 * @date 2020-07-13
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "global.h"
#include "serial_driver.h"

#include<iostream>
#include<fstream>

BUFFER_t g_serial_buffer;


void SplitString(const std::string& s, std::vector<std::string>& v, const std::string& c)
{
  std::string::size_type pos1, pos2;
  pos2 = s.find(c);
  pos1 = 0;
  while(std::string::npos != pos2)
  {
    v.push_back(s.substr(pos1, pos2-pos1));

    pos1 = pos2 + c.size();
    pos2 = s.find(c, pos1);
  }
  if(pos1 != s.length())
  {
    v.push_back(s.substr(pos1));
  }
}

storm_car::car_status AnalysisData(char * param)
{
  storm_car::car_status result;
  std::string s(param);
  std::vector<std::string> v;

  SplitString(s, v, ",");
  ROS_INFO("Heading is %s",v[3].c_str());
  ROS_INFO("Lattitude is %s",v[12].c_str());
  ROS_INFO("Longitude is %s",v[13].c_str());
  ROS_INFO("V is %s",v[18].c_str());
  ROS_INFO("Status is %s",v[21].c_str());
  result.heading = StringToDouble(v[3].c_str());
  result.lattitude = StringToDouble(v[12].c_str());
  result.longitude = StringToDouble(v[13].c_str());
  result.velocity = StringToDouble(v[18].c_str());

  return result;
}

double StringToDouble(std::string param)
{
  std::vector<std::string> v;
  double result, l, r;

  SplitString(param, v, ".");
  l = atof(v[0].c_str());
  r = atof(v[1].c_str());

  for(int i = 0; i < v[1].length(); ++ i)
  {
    r /= 10;
  }

  result = l + r;
  
  return result;
}

std::string DoubleToString(double param, uint8_t length)
{
  int64_t integer  = (int64_t)param;
  double temp  = (param - integer) * pow10l(length+1);

  int64_t decimal = (int64_t)temp;
  std::string decimal_str = std::to_string(decimal);

  std::string result;

  result = std::to_string(integer) + ".";

  for (uint8_t i = 0; i < length+1 - decimal_str.length(); i++)
  {
    result += "0";
  }
  
  result += std::to_string(decimal);
  
  return result;
}

void BufferInit()
{
  g_serial_buffer.WRHead = 0;
  g_serial_buffer.RDTail = 0;
  memset(g_serial_buffer.Buffer, 0x00, sizeof(g_serial_buffer.Buffer));
}

void BufferPrintf()
{
  for(int i = 0; i < sizeof(g_serial_buffer.Buffer); ++ i)
  {
    if(0 == i%8)
    {
      printf("\n");
    }
    printf("[0x%x] ",g_serial_buffer.Buffer[i]);
  }
}

void ArrayPrintf(char * src, uint16_t len)
{
  for(int i = 0; i < len; ++ i)
  {
    if(0 == i%8)
    {
      printf("\n");
    }
    printf("[0x%x] ",src[i]);
  }
}

uint8_t WriteToBuffer(BUFFER_t * dest, char * src, uint16_t len)
{
  uint16_t buffer_aviable_size = 0;

  if(dest->WRHead >= dest->RDTail)
  {
    buffer_aviable_size = BUFFERSIZE - (dest->WRHead - dest->RDTail);
    // printf("\nbuffer_aviable_size is %d\n", buffer_aviable_size);
    
    if(buffer_aviable_size <= len)
    {
      // printf("\ndest->WRHead >= dest->RDTail\n");
      return 0;
    }
    if((dest->WRHead + len) <= BUFFERSIZE)
    {
      memcpy(dest->Buffer + dest->WRHead, src, len);
    }
    else
    {
      memcpy(dest->Buffer + dest->WRHead, src, BUFFERSIZE - dest->WRHead);
      memcpy(dest->Buffer, src + BUFFERSIZE - dest->WRHead, dest->WRHead + len - BUFFERSIZE);
    }  
  }
  else
  {
    buffer_aviable_size = dest->RDTail - dest->WRHead;
    // printf("\nbuffer_aviable_size is %d\n", buffer_aviable_size);
    if(buffer_aviable_size <= len)
    {
      // printf("\ndest->WRHead < dest->RDTail\n");
      return 0;
    }
    memcpy(dest->Buffer + dest->WRHead, src, len);
  }
  dest->WRHead += len;
  dest->WRHead %= BUFFERSIZE;
  // printf("\nWRHead is %d,RDTail is %d,len is %d\n", dest->WRHead, dest->RDTail, len);
  
  return len;
}

uint8_t ReadFromBuffer(char * dest, BUFFER_t * src, uint16_t len)
{
  uint16_t dest_tail, dest_head;
  uint16_t i = src->RDTail;

  if(src->WRHead == src->RDTail)
  {
    return 0;
  }

  while(('$' != src->Buffer[i]) && (i != src->WRHead))
  {
    i ++;
    i %= BUFFERSIZE;
  }
  dest_tail = i;
  
  while((0x0A != src->Buffer[i]) && (i != src->WRHead))
  {
    i ++;
    i %= BUFFERSIZE;
  }
  dest_head = i;

  if((0x0A != src->Buffer[i]) && (dest_head == src->WRHead))
  {
    return 0;
  }

  // printf("\ndest_tail is located at %d\ndest_head is located at %d\n", dest_tail, dest_head);

  memset(dest, 0x00, len);

  if(dest_head > dest_tail)
  {
    memcpy(dest, src->Buffer + dest_tail, dest_head - dest_tail + 1);
    // printf("\nWRHead is %d,RDTail is %d\n", src->WRHead, src->RDTail);
    src->Buffer[dest_head] = 0x00;
    src->RDTail = dest_head;
    return (dest_head - dest_tail + 1);
  }
  else
  {
    memcpy(dest, src->Buffer + dest_tail, BUFFERSIZE - dest_tail);
    memcpy(dest + BUFFERSIZE - dest_tail, src->Buffer, dest_head + 1);
    // printf("\nWRHead is %d,RDTail is %d\n", src->WRHead, src->RDTail);
    src->Buffer[dest_head] = 0x00;
    src->RDTail = dest_head;
    return (BUFFERSIZE - dest_tail + dest_head + 1);
  }
}

bool WriteToFile(char * param)
{
  std::ofstream fout(FILEPATH, std::ios::app);
  if(!fout)
  {
    ROS_INFO("OPEN FILE FAILED");
    return false;
  } 
  fout<< param <<std::endl;
  fout.close();
  return true;
}

bool WriteToFile(storm_car::car_status param)
{
  static double err = 0.0000000;
  std::ofstream fout(FILEPATH, std::ios::app);
  if(!fout)
  {
    ROS_INFO("OPEN FILE FAILED");
    return false;
  }

  // err += 0.0000001;

  std::string heading = DoubleToString(param.heading, 2);
  std::string lattitude = DoubleToString(param.lattitude + err, 8);
  std::string longitude = DoubleToString(param.longitude + err, 8);
  std::string velocity = DoubleToString(param.velocity, 3);
  fout << '$';
  fout << heading << ',' << lattitude << ',' << longitude << ',' << velocity <<std::endl;
  fout.close();
  return true;
}

std::string ReadFromFile()
{
  std::string temp;
  std::ifstream fin(FILEPATH, std::ios::in);
  
  if(!fin)
  {
    ROS_INFO("OPEN FILE FAILED");
    return "error";
  }
  else
  {
    for (uint8_t i = 0; i < 10; i++)
    {
      fin >> temp;
      // std::getline(fin, temp);
      ROS_INFO("read data is %s", temp.c_str());
    }
    
    return temp;
  }
}